//------------------------------------------------------------------------------
//  <copyright file="FRIdispatcher.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#pragma once
#include <ControllerStateMachineRegisters.h>
#include "IFRIConnection.h"

namespace Microsoft 
{
    namespace Robotics
    {
        namespace Hardware
        {
            namespace KukaLBR
            {

                // Interop callbacks to managed FRI interface with no parameters
                typedef int (__stdcall *FRICALLBACK0)();

                // Interop callbacks to managed FRI interface with 1 parameters
                typedef int (__stdcall *FRICALLBACKPD1)(double *);

                // Interop callbacks to managed FRI interface with 1 double * and 1 int parameter
                typedef int (__stdcall *FRICALLBACKPD1I1)(double *, int);

                // Interop callbacks to managed FRI interface with 1 parameter as array of pointers
                typedef int (__stdcall *FRICALLBACKPPD1)(double **);

                // Interop callbacks to managed FRI interface with 4 parameters
                typedef int (__stdcall *FRICALLBACKPD4)(double *, double *, double *, double *);

                // DIspatches calls to the managed FRI control
                class FRIdispatcher : public IFRIConnection
                {
                public:
                    /// <summary> Initializes a new instance of the <c>FRIdispatcher</c> for dispatching callbacks to a managed wrapper.
                    /// This class is not used for unmanaged FRI implementations. When there is a stable unamanged FRI implementation this can be
                    /// move to Test.Robotics.Manipulation </summary>
                    /// <param name="pfnInitializeFri"> Callback to initialize the FRI</param>
                    /// <param name="pfnCloseFri">Callback to  to close the FRI  </param>
                    /// <param name="pfnWaitFri">Callback to wait on FRI connection </param>
                    /// <param name="pfnDoNothingFri">Callback to do nothing  </param>
                    /// <param name="pfnGetRequestFri">Callback to get FRI request  </param>
                    /// <param name="pfnJointControlFr">Callback to do joint control  </param>
                    /// <param name="pfnJointImpedanceControlFri">Callback for impeance control </param>
                    /// <param name="pfnCartesianImpedanceControlFri">Callback for cartesian control  </param>
					/// <param name="pfnKrlCartesianImpedanceControlFri">Callback for cartesian control using Krl motion interpreter</param>
                    FRIdispatcher(
                        FRICALLBACK0 pfnInitializeFri,
                        FRICALLBACK0 pfnCloseFri,
                        FRICALLBACK0 pfnWaitFri,
                        FRICALLBACK0 pfnDoNothingFri,
                        FRICALLBACKPPD1 pfnGetRequestFri,
                        FRICALLBACKPD1 pfnJointControlFri,
                        FRICALLBACKPD4 pfnJointImpedanceControlFri,
						FRICALLBACKPD4 pfnCartesianImpedanceControlFri,
						FRICALLBACKPD1I1 pfnDoKrlCartesianImpedanceControlFri);

                    /// <summary> destructor </summary>
                    ~FRIdispatcher();

                    /// <summary> Initializes a new instance of the <c>FRIdispatcher</c> class. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int InitializeFRI();

                    /// <summary> Shut down the FRI server. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int CloseFRI();

                    /// <summary> Wait for an FRI client to connect. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int WaitOnFRIConnect();

                    /// <summary> Wait on a new FRI request. </summary>
                    /// <returns> State of the LBR as indicated by the next FRI request. </returns>
                    virtual int GetFRIRequest(double **ppFeedbackRegisters);

                    /// <summary> Send a response to the FRI request, but take no control actions in it. </summary>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int DoNothing();

                    /// <summary> Execute joint position control. </summary>
                    /// <param name="jointPosition"> Array of joint positions. </param>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int DoPositionControl(double *jointPosition);

                    /// <summary> Gets the quality of the FRI connection </summary>
                    /// <returns> The quality of the FRI connection. </returns>
                    virtual FRIQuality GetFRIQuality();

                    /// <summary> Execute joint impedance control. </summary>
                    /// <param name="jointPosition"> Array of joint positions. </param>
                    /// <param name="jointStiffness"> Array of joint stiffness values. </param>
                    /// <param name="jointDamping"> Array of joint damping values. </param>
                    /// <param name="jointAdditionalTorque"> Array of addition joint torque values. </param>
                    /// <returns> Status code.  0 on no error. </returns>
                    virtual int DoJointImpedanceControl(
                        double *jointPosition,
                        double *jointStiffness,
                        double *jointDamping,
                        double *jointAdditionalTorque);

                    /// <summary> Execute Cartesian impedance control. </summary>
                    /// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
                    /// <param name="cartesianStiffness"> Array of Cartesian stiffness values. </param>
                    /// <param name="cartesianDamping"> Array of Cartesian damping values. </param>
                    /// <param name="additionalTCPForceTorque"> Array of additional TCP force torque values. </param>
                    virtual int DoCartesianImpedanceControl(
                        double *cartesianPosition,
                        double *cartesianStiffness,
                        double *cartesianDamping,
                        double *additionalTCPForceTorque);

					/// <summary> Execute Cartesian impedance control by passing position goals to KRL script. </summary>
					/// <param name="pCartesianPosition"> Array indicating desired cartesian position of TCP (tool control point) relative to base frame. </param>
					/// <param name="sequenceNumber"> Used to identify when a new position command is ready to be executed. </param>
					virtual int DoKrlCartesianImpedanceControl(double *pCartesianPosition, int sequenceNumber);

                private:
                    /// <summary> Array of pointers to arrays containg feedback values. </summary>
                    ControllerStateMachineRegisters<double> *pRegisters;

                    /// <summary> Initialize callback </summary>
                    FRICALLBACK0  pfnInitializeFri;

                    /// <summary> Close callback </summary>
                    FRICALLBACK0       pfnCloseFri;

                    /// <summary> Blocking wait call back  </summary>
                    FRICALLBACK0        pfnWaitFri;

                    /// <summary> Do Nothing call back  </summary>
                    FRICALLBACK0   pfnDoNothingFri;

                    /// <summary> Wait on a new FRI request. </summary>
                    FRICALLBACKPPD1 pfnGetRequestFri;

                    /// <summary> Do joint position control </summary>
                    FRICALLBACKPD1  pfnJointControlFri;

                    /// <summary> Execute joint impedance control. </summary>
                    FRICALLBACKPD4   pfnJointImpedanceControlFri;

                    /// <summary> Execute Cartesian impedance control. </summary>
                    FRICALLBACKPD4   pfnCartesianImpedanceControlFri;

                    /// <summary> Execute Cartesian impedance control using KRL generated motions. </summary>
					FRICALLBACKPD1I1 pfnKrlCartesianImpedanceControlFri;
                };
            }
        }
    }
}
